#include "preprocess.h"
namespace perception
{
    namespace lidar_preprocess
    {

        int PreProcess::Process(LidarDataInType &data_in, LidarPoint &no_ground_points, LidarPoint &ground_points,
                                config &config_)
        {
            int ret = 0;
            if (config_.method == 1)
                ret = PreprocessMulti(data_in, no_ground_points, ground_points, config_);
            // else if (config_.method == 2)
            //     ret = PreprocessPatchWork(data_in, no_ground_points, ground_points, config_);
            return ret;
        }
        int PreProcess::PreprocessSingle(pcl::PointCloud<XYZI_1>::Ptr &data_in, pcl::PointCloud<XYZI_1>::Ptr &no_ground_points,
                                         config &config_)
        {
            if (data_in->points.empty())
            {
                RCLCPP_WARN(private_nh_->get_logger(), "no points input");
                return -2;
            }
            pcl::PointCloud<XYZI_1>::Ptr data_out_re(new pcl::PointCloud<XYZI_1>);
            // 1.格式转换 将输入点云格式转换为输出点云格式
            pcl::copyPointCloud(*data_in, *data_out_re);
            pcl::PointCloud<XYZI_1>::Ptr data_out_re2(new pcl::PointCloud<XYZI_1>);
            pcl::PointCloud<XYZI_1>::Ptr data_out_co1(new pcl::PointCloud<XYZI_1>);
            pcl::PointCloud<XYZI_1>::Ptr data_out_co2(new pcl::PointCloud<XYZI_1>);
            pcl::PointCloud<XYZI_1>::Ptr data_out_ra(new pcl::PointCloud<XYZI_1>);
            pcl::PointCloud<XYZI_1>::Ptr data_out_vo(new pcl::PointCloud<XYZI_1>);
            // pcl::PointCloud<PointXYZIVHRTL>::Ptr data_out_se(new pcl::PointCloud<PointXYZIVHRTL>);
            preprocess1->Process(data_out_re, data_out_re2, config_);
            config_.condition_filter_config_.OrAnd = true;
            preprocess2->Process(data_out_re2, data_out_co1, config_);
            config_.condition_filter_config_.OrAnd = false;
            preprocess2->Process(data_out_co1, data_out_co2, config_);
            preprocess4->Process(data_out_co2, data_out_vo, config_);
            preprocess5->Process(data_out_vo, no_ground_points, config_.average_seg_config_);
            if (no_ground_points->points.empty())
                return -1;
            groud_points_all->points.insert(groud_points_all->points.end(), data_out_vo->points.begin(),
                                            data_out_vo->points.end());
            return 0;
        }

        int PreProcess::PreprocessMulti(LidarDataInType &data_in, LidarPoint &no_ground_points, LidarPoint &ground_points,
                                        config &config_)
        {
            // 1.得到哪些雷达收到数据 存入map
            auto now_time =
                std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
            auto master_lidar_time = data_in.point_cloud_ptr_map[0].time;
            // 1.1判断雷达数据是否超时,超时则不再处理,只根据主雷达判断
            if (debug == false)
            {
                if ((now_time.count() * 1000 - master_lidar_time) * 1000 > config_.master_sys_time_eps)
                {
                    RCLCPP_ERROR(private_nh_->get_logger(), "超时：: %f ms", (now_time.count() * 1000 - master_lidar_time) * 1000);
                    return 1;
                }
            }
            // std::cout << "data_in.point_cloud_ptr_map[0].points->points.size():" <<
            // data_in.point_cloud_ptr_map[0].points->points.size() << std::endl;
            //  pcl::PointCloud<XYZI_1>::Ptr cloud(new pcl::PointCloud<XYZI_1>);
            // pcl::copyPointCloud(*data_in.point_cloud_ptr_map[0].points, *cloud);
            // std::cout << "cloud->points.size():" << cloud->points.size() << std::endl;
            // data_in_all[0] = cloud;
            // 1.2统计哪几个雷达有消息,且和主雷达时间不超过差30ms
            for (int i = 0; i < data_in.point_cloud_ptr_map.size(); i++)
            {
                if (std::fabs(master_lidar_time - data_in.point_cloud_ptr_map[i].time) * 1000 < config_.master_slave_time_eps &&
                    (!data_in.point_cloud_ptr_map[i].points->points.empty()))
                {
                    lidar_map[lidar_map.size()] = i;
                    //  pcl::PointCloud<XYZI_1>::Ptr cloud(new pcl::PointCloud<XYZI_1>);
                    // pcl::copyPointCloud(*data_in.point_cloud_ptr_map[i].points, *cloud);
                    // data_in_all[i] = cloud;
                }
                else
                    RCLCPP_WARN(private_nh_->get_logger(), "lidar %d no points", i);
            }
            // 2.分割每个雷达点云,并预处理
            int counter = 0; // 用于统计分派每一个雷达分割之后分了几个线程去分配计算
            for (int i = 0; i < lidar_map.size(); i++)
            {
                // 2.1分割点云
                // std::cout << "data_in.point_cloud_ptr_map[lidar_map[i]].points:" <<
                // data_in.point_cloud_ptr_map[lidar_map[i]].points->points.size() << std::endl;
                MultiProcessPoints_->SplitPoints<XYZI_1, XYZI_1, multi_process_points_config>(
                    data_in.point_cloud_ptr_map[lidar_map[i]].points, data_in_all, config_.multi_process_points_config_);
                int start_num = 0; // 用于判断data_in_all从哪一个结束是无点云
                for (; start_num < data_in_all.size(); start_num++)
                {
                    if (data_in_all[start_num]->points.empty())
                        break;
                }
                if (data_in_all.size() > counter)
                {
                    // 2.2 多线程处理
                    for (int j = counter; j < start_num; j++)
                    {
                        if (data_in_all[j]->points.empty())
                            continue;
                        // std::cout << "data_in_all[j]->points.:" << data_in_all[j]->points.size() << std::endl;
                        int th = (i + 1) * 20 + j;
                        // int th = (1) * 20 + j;
                        auto config_tmp = config_;
                        auto SET_NAME(name, th) = [&]()
                        {
                            // std::mutex SET_NAME(name1, th);
                            // std::lock_guard<std::mutex> locker(SET_NAME(name1, th));
                            auto result = this->PreprocessSingle(data_in_all[j], data_out_all[j], config_tmp);
                            return result;
                        };
                        std::future<decltype(this->PreprocessSingle(data_in_all[j], data_out_all[j], config_tmp))> SET_NAME(name,
                                                                                                                            counter);
                        if (executor->idlCount() > 0)
                        {
                            SET_NAME(name, counter) = executor->commit(SET_NAME(name, th));
                            thread_name.emplace_back(std::move(SET_NAME(name, counter)));
                            counter++;
                            usleep(1000); // 为了使线程成功初始化，需要一点延时,现在是0.1ms
                        }
                    }
                }
            }
            // 3.结束线程,合并点云
            // std::cout << "counter:" << counter << std::endl;
            for (int i = counter - 1; i >= 0; i--)
            {
                std::future_status status;
                status = thread_name[i].wait_for(std::chrono::seconds(0));
                while (!(status == std::future_status::ready))
                {
                    status = thread_name[i].wait_for(std::chrono::seconds(0));
                }
                auto a = thread_name[i].get();
            }
            // std::cout << "counter1:" << counter << std::endl;
            MultiProcessPoints_->MergePoints<XYZI_1, XYZI_1>(data_out_all, no_ground_points.points);
            no_ground_points.time = data_in.point_cloud_ptr_map[0].time;
            ground_points.time = data_in.point_cloud_ptr_map[0].time;
            // ground_points.points = groud_points_all;
            ground_points.points->points.insert(ground_points.points->points.end(), groud_points_all->points.begin(), groud_points_all->points.end());
            RCLCPP_INFO(private_nh_->get_logger(), "no_ground_points.points num:%d", no_ground_points.points->points.size());
            RCLCPP_INFO(private_nh_->get_logger(), "ground_points.points num:%d", ground_points.points->points.size());
            // 4.回归变量初始化状态
            lidar_map.clear();
            thread_name.clear();
            groud_points_all->points.clear();
            for (int i = 0; i < counter; i++)
            {
                data_out_all[i]->points.clear();
                data_in_all[i]->points.clear();
            }
            return 0;
        }
        // int PreProcess::PreprocessPatchWork(LidarDataInType &data_in, LidarPoint &no_ground_points, LidarPoint
        // &ground_points, config &config_)
        // {
        //     // 1.得到哪些雷达收到数据 存入map
        //     // 1.得到哪些雷达收到数据 存入map
        //     // 所有lidar一起
        //     XYZI_PTR tmp;
        //     tmp.reset(new XYZI);

        //     double now_time = ros::Time::now().toSec();
        //     double end_time = 0;
        //     auto master_lidar_time = data_in.point_cloud_ptr_map[0].time;
        //     // 1.1判断雷达数据是否超时,超时则不再处理,只根据主雷达判断

        //     if (debug == false)
        //     {
        //         if ((now_time - master_lidar_time) * 1000 > 200)
        //         {
        //             std::cout << "超时：" << (now_time - master_lidar_time) * 1000 << "ms";
        //             return 1;
        //         }
        //     }
        //     tmp->points.insert(tmp->points.end(), data_in.point_cloud_ptr_map[0].points->points.begin(),
        //     data_in.point_cloud_ptr_map[0].points->points.end());
        //     // 1.2统计哪几个雷达有消息,且和主雷达时间不超过差30ms
        //     for (int i = 0; i < data_in.point_cloud_ptr_map.size(); i++)
        //     {
        //         if (std::fabs(master_lidar_time - data_in.point_cloud_ptr_map[i].time) * 1000 < 30 &&
        //             (!data_in.point_cloud_ptr_map[i].points->points.empty()))
        //         {
        //             lidar_map[lidar_map.size()] = i;
        //             tmp->points.insert(tmp->points.end(), data_in.point_cloud_ptr_map[i].points->points.begin(),
        //             data_in.point_cloud_ptr_map[i].points->points.end());
        //         }
        //         else
        //             std::cout << "no lidar" << i << " data\n";
        //     }
        //     pcl::PointCloud<XYZI_1> pc_curr;
        //     pcl::PointCloud<XYZI_1> pc_ground;
        //     pcl::PointCloud<XYZI_1> pc_non_ground;
        //     pcl::PointCloud<XYZI_1>::Ptr data_out_re(new pcl::PointCloud<XYZI_1>);
        //     // 1.格式转换 将输入点云格式转换为输出点云格式

        //     pcl::copyPointCloud(*tmp, *data_out_re);
        //     double now_time1 = ros::Time::now().toSec();
        //     //  pcl::PointCloud<XYZI_1>::Ptr data_out_vo(new pcl::PointCloud<XYZI_1>);
        //     pcl::PointCloud<XYZI_1>::Ptr data_out_vo1(new pcl::PointCloud<XYZI_1>);

        //     pcl::PointCloud<XYZI_1>::Ptr data_out_re2(new pcl::PointCloud<XYZI_1>);
        //     pcl::PointCloud<XYZI_1>::Ptr data_out_co1(new pcl::PointCloud<XYZI_1>);
        //     pcl::PointCloud<XYZI_1>::Ptr data_out_co2(new pcl::PointCloud<XYZI_1>);
        //     pcl::PointCloud<XYZI_1>::Ptr data_out_ra(new pcl::PointCloud<XYZI_1>);
        //     pcl::PointCloud<XYZI_1>::Ptr data_out_vo(new pcl::PointCloud<XYZI_1>);
        //     // pcl::PointCloud<PointXYZIVHRTL>::Ptr data_out_se(new pcl::PointCloud<PointXYZIVHRTL>);
        //     // preprocess1->Process(data_out_re, data_out_re2, config_);
        //     // config_.condition_filter_config_.OrAnd = true;
        //     // preprocess2->Process(data_out_re2, data_out_co1, config_);
        //     // config_.condition_filter_config_.OrAnd = false;
        //     double now_time2 = ros::Time::now().toSec();
        //     // preprocess4->Process(data_out_co1, data_out_vo, config_);
        //     preprocess4->Process(data_out_re, data_out_vo, config_);
        //     double now_time3 = ros::Time::now().toSec();
        //     // pcl::copyPointCloud(*data_in.point_cloud_ptr_map.find(i)->second, pc_curr);
        //     // pc_curr = *data_out_re;
        //     pc_curr = *data_out_vo;
        //     double time_taken;
        //     PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken);
        //     // WARN("pc_non_ground {} size", pc_non_ground.points.size());

        //     data_out_all.emplace_back(pc_non_ground.makeShared());
        //     pc_curr.points.clear();

        //     MultiProcessPoints_->MergePoints<XYZI_1, XYZI_1>(data_out_all, no_ground_points.points);
        //     no_ground_points.time = data_in.point_cloud_ptr_map[0].time;
        //     ground_points.time = data_in.point_cloud_ptr_map[0].time;
        //     ground_points.points = pc_ground.makeShared();

        //     for (int i = 0; i < data_out_all.size(); i++)
        //     {
        //         data_out_all[i]->points.clear();
        //     }
        //     // data_out = pc_non_ground.makeShared();
        //     return 0;
        // }
    } // namespace lidar_preprocess
} // namespace perception